﻿#region License
/**	
The MIT License (MIT)

Copyright (c) 2013 Philipp Schillinger, Michael Alekseew, Andreas Braun, Steeven Zeiß

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE. 
 **/
#endregion

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Threading.Tasks;

namespace IndoorLocalization {

	/// <summary>
	/// Manages the surroundings.
	/// </summary>
	/*static*/ class FloorPlan {

		public delegate void ProgressCallback(int global, int generation);

		public delegate void SuccessCallback(int newBest);

		/// <summary>
		/// The colors used for the coverage map.
		/// 0: Not covered
		/// 1: Covered by 1 cam
		/// 2: Covered by 2 cams
		/// 3: Covered by 3 or more cams
		/// </summary>
		private static Color[] COV_COLORS = 
		{ Color.Red, Color.Orange, Color.Yellow, Color.GreenYellow };

		private static Bitmap map;
		/// <summary>
		/// The map which describes this floor plan.
		/// </summary>
		public static Bitmap Map {
			get { return map; }
			set {
				map = value;
				if (value != null) {
					// calculate borders
					maxX = value.Width - 1;
					maxY = value.Height - 1;
					// retreive brightness and store it in a seperate array
					// this is neccessary to use parallel calls on the map
					brightness = new float[(maxX + 1) * (maxY + 1)];
					lock (map) {
						for (int i = 0; i < maxX * maxY; i++) {
							int x = i % maxX;
							int y = i / maxX;
							brightness[i] = map.GetPixel(x, maxY - y).GetBrightness();
						}
					}
					// initialize coverage
					coverageValues = new int[(maxX + 1) * (maxY + 1)];
					InitializeMapCoverage();
				} else {
					maxX = -1;
					maxY = -1;
				}
			}
		}

		/// <summary>
		/// The coverage value of each pixel.
		/// </summary>
		private static int[] coverageValues;

		/// <summary>
		/// The brightness value of each pixel.
		/// </summary>
		private static float[] brightness;

		private static int maxX;
		/// <summary>
		/// The maximum value for x.
		/// </summary>
		public static int MaxX { get { return maxX; } }

		private static int maxY;
		/// <summary>
		/// The maximum value for y.
		/// </summary>
		public static int MaxY { get { return maxY; } }

		/// <summary>
		/// Determines if the given position is in bounds and there is no wall.
		/// </summary>
		/// <param name="position">Position to be tested.</param>
		/// <returns><c>true</c> if position is valid, else <c>false</c>.</returns>
		public static bool IsValidPosition(HCoord position) {
			// test if out of bounds
			if (!InBounds(position))
				return false;
			// test if there is a wall
			if (IsWall(position.X, position.Y))
				return false;
			// seems to be valid
			return true;
		}

		/// <summary>
		/// Determines if the given point is in bounds.
		/// </summary>
		/// <param name="position"></param>
		/// <returns></returns>
		public static bool InBounds(HCoord position) {
			return !(position.X > maxX || position.Y > maxY || position.X < 0 || position.Y < 0);
		}

		/// <summary>
		/// Determines if there is a wall at this position.
		/// Position has to be on the map.
		/// </summary>
		/// <param name="x">x value of the position.</param>
		/// <param name="y">y value of the position.</param>
		/// <returns><c>true</c> if there is a wall, else <c>false</c>.</returns>
		public static bool IsWall(int x, int y) {
			if (map == null) return false;

			int i = y * maxX + x;
			return brightness[i] < Program.WallThreshold;
		}

		/// <summary>
		/// Determines of there is a wall in the direct connection line between the given points.
		/// </summary>
		/// <param name="point1">First point.</param>
		/// <param name="point2">Second point.</param>
		/// <returns><c>true</c> if there is a wall in between, else <c>false</c>.</returns>
		public static bool IsWallBetween(HCoord point1, HCoord point2) {
			if (map == null) return false;

			// determine how many iterations are neccessary
			int length = Math.Max(Math.Abs(point1.X - point2.X), Math.Abs(point1.Y - point2.Y));
			bool lengthIsX = length == Math.Abs(point1.X - point2.X);

			// determine which point is the "small" one and the scale factor
			HCoord p1, p2;
			int scale;
			if (lengthIsX) {
				p1 = (point1.X < point2.X) ? point1 : point2;
				p2 = (p1 == point2) ? point1 : point2;
				scale = p2.Y - p1.Y;
			} else {
				p1 = (point1.Y < point2.Y) ? point1 : point2;
				p2 = (p1 == point2) ? point1 : point2;
				scale = p2.X - p1.X;
			}

			// determine the best possible multiplicator to use for iteration
			// a multiplicator is used for speed-up the calculation
			// it more than doubles the average performance
			int multi = 1;
			if (length % 3 != 0) multi = 3;
			else if (length % 2 != 0) multi = 4;
			else if (length % 5 != 0) multi = 5;
			else if (length % 7 != 0) multi = 7;
			else if (length % 11 != 0) multi = 11;

			// iterate over all pixels in between and check if one of them is a wall
			for (int index = 1; index < length; index++) {
				int i = index*multi % length;
				if (lengthIsX && IsWall(p1.X + i, p1.Y + (int)Math.Round(i / (float)length * scale))
					|| !lengthIsX && IsWall(p1.X + (int)Math.Round(i / (float)length * scale), p1.Y + i))
					return true;
			}

			// no wall detected
			return false;
		}

		/// <summary>
		/// Determines how many sensor cams cover a certain point on the map.
		/// </summary>
		/// <param name="point">A point on the map.</param>
		/// <returns>A number of how many cams cover this point.</returns>
		public static int PointCoverage(HCoord point) {
			if(!IsValidPosition(point)) return 0;
			int result = 0;

			// test each of the cams for this position
			foreach (SensorCam cam in Program.Cameras) {
				result += PointCoverage(point, cam);
			}

			return result;
		}

		/// <summary>
		/// Determines if the given cam covers a certain point on the map.
		/// The position is considered as valid, so test it before.
		/// </summary>
		/// <param name="point">A point on the map.</param>
		/// <param name="cam">The cam to test.</param>
		/// <returns>1 if covered, 0 if not (returns int to add several values).</returns>
		public static int PointCoverage(HCoord point, SensorCam cam) {
			// determine cam positioning
			HCoord camPos = new HCoord(cam.X, cam.Y);
			double angle = Math.Atan2(point.Y - cam.Y, point.X - cam.X) * 180 / Math.PI;

			// covered if point is in the right direction and there is no wall in between
			if ((Math.Abs(angle - cam.Orientation) < Program.CamAperture / 2
					|| Math.Abs(angle - (cam.Orientation - 360)) < Program.CamAperture / 2)
				&& !IsWallBetween(camPos, point))

					return 1;

			return 0;
		}

		/// <summary>
		/// Updates a value map indicating which points are covered how much.
		/// This method will only consider the given cam.
		/// </summary>
		/// <param name="cam">The cam that should be updated.</param>
		/// <param name="add"><c>true</c> if the cam was added, <c>false</c> if removed.</param>
		public static void UpdateCamAtCoverageMap(SensorCam cam, bool add) {
			int width = map.Width;

			// update the map in parallel
			Parallel.For(0, map.Height * map.Width, i => {
				int x = i % width;
				int y = i / width;
				if (!IsWall(x, y)) {
					// update sum of covering cams
					coverageValues[i] = Math.Min(3, add? 
						coverageValues[i]+PointCoverage(new HCoord(x, y), cam)
						: coverageValues[i]-PointCoverage(new HCoord(x, y), cam));
				}
			});
		}

		/// <summary>
		/// Draws (just draws not calculates) the coverage map corresponding to the last known values.
		/// </summary>
		/// <returns>A coverage bitmap to be displayed.</returns>
		public static Bitmap DrawCoverageMap() {
			Bitmap coverageMap = new Bitmap(map);

			// draw pixels depending on their entry in the coverage value map
			for (int i = 0; i < coverageMap.Height * coverageMap.Width; i++) {
				int x = i % coverageMap.Width;
				int y = i / coverageMap.Width;
				if (!IsWall(x, y)) {
					coverageMap.SetPixel(x, coverageMap.Height - y - 1, COV_COLORS[Math.Max(0, coverageValues[i])]);
				} else {
					coverageMap.SetPixel(x, coverageMap.Height - y - 1, Color.Black);
				}
			}

			return coverageMap;
		}

		/// <summary>
		/// Initializes the coverage values.
		/// </summary>
		public static void InitializeMapCoverage() {
			if (map == null) return;
			int width = map.Width;

			// initialize the map in parallel
			Parallel.For(0, map.Height * map.Width, i => {
				int x = i % width;
				int y = i / width;
				if (!IsWall(x, y)) {
					coverageValues[i] = Math.Min(3, PointCoverage(new HCoord(x, y)));
				}
			});
		}

		/// <summary>
		/// Calculates how many percent of the map are covered by 0, 1, 2 or more cams.
		/// </summary>
		/// <returns>Array with 4 values, containing the percentages of 0, 1, 2 and more cams.</returns>
		public static int[] CalcCoveragePercentages() {
			int[] percentages = new int[]{0,0,0,0};

			// sum up the occurence of each value
			// walls will be ignored
			int width = map.Width;
			int sumOfFields = 0;
			Parallel.For(0, map.Height * map.Width, i => {
				int x = i % width;
				int y = i / width;
				if (!IsWall(x, y)) {
                    lock (percentages) {
                        percentages[coverageValues[i]]++;
                        sumOfFields++;
                    }
				}
			});
			// calculate percentage
			for (int i = 0; i < percentages.Length; i++)
				percentages[i] = (int)Math.Round(percentages[i] * 100.0f / sumOfFields);

			return percentages;
		}


        /// <summary>
        /// Calculates the coverage map for the given list of cams in a certain region of interest.
        /// </summary>
        /// <param name="camList">The cameras used for the calculation of the coverage map.</param>
        /// <param name="roi">The region of interest to calculate the coverage for.</param>
        /// <returns>The calculated coverage map as an array of pixel coverage values.</returns>
		private static int[] CalcCoverageMap(List<SensorCam> camList, Rectangle roi) {
			int width = roi.Width;
			SensorCam[] cams = new SensorCam[camList.Count];
			camList.CopyTo(cams);
			int[] coverageMap = new int[roi.Height * roi.Width];

			// update the map in parallel
			Parallel.For(0, coverageMap.Length * cams.Length, i => {
				int index = i % coverageMap.Length;
				int x = index % width + roi.X;
				int y = index / width + roi.Y;
				int c = i / coverageMap.Length;
				if (!IsWall(x, y)) {
					// update sum of covering cams
					int pointCov = PointCoverage(new HCoord(x, y), cams[c]);
                    lock (coverageMap) coverageMap[index] += pointCov;
				}
			});

			return coverageMap;
		}

        /// <summary>
        /// Calculates the coverage value (which means the number of covered pixels) of the given coverage map.
        /// </summary>
        /// <param name="coverageMap">The coverage map to use.</param>
        /// <param name="roi">The region of interest used for the coverage map.</param>
        /// <returns>The number of covered pixels.</returns>
		private static int CalcCoverageValue(int[] coverageMap, Rectangle roi) {
			int width = roi.Width;
			int value = 0;
            object lockObj = new object();

			Parallel.For(0, coverageMap.Length, i => {
				int x = i % width + roi.X;
				int y = i / width + roi.Y;
				if (coverageMap[i] >= 2) 
                    lock(lockObj) value++;
			});

			return value;
		}

        /// <summary>
        /// Optimizes the placement of the given cameras in a certain region of interest.
        /// Is able to update a GUI with the current progress and success.
        /// </summary>
        /// <param name="initialCams">A list of the cams to be optimized.</param>
        /// <param name="progCB">A callback function to update the current progress.</param>
        /// <param name="succCB">A callback function to update a success.</param>
        /// <param name="steps">The number of steps (generations) to be done.</param>
        /// <param name="childFactor">The number of children each parent will generate.</param>
        /// <param name="groups">The number of independent groups.</param>
        /// <param name="roi">The region of interest to be optimized.</param>
		public static void Optimize(ref List<SensorCam> initialCams, ProgressCallback progCB, SuccessCallback succCB, int steps, int childFactor, int groups, Rectangle roi) {
			Program.CamPause();
			// Algorithmus parameters
			int genCount = initialCams.Count;
			int[][] sigma = new int[groups][];

			// Reference value
			int[] refValue = new int[groups];
			for (int i = 0; i < groups; i++) {
				sigma[i] = new int[] { 15, 15, 15 };
				refValue[i] = CalcCoverageValue(CalcCoverageMap(initialCams, roi), roi);
			}
			Program.Log("Start value: " + refValue[0]);
			succCB(refValue[0]);

			// Initialization
			List<SensorCam>[] parentGeneration = new List<SensorCam>[groups];
			List<SensorCam>[] childGeneration = new List<SensorCam>[groups];
			for (int i = 0; i < groups; i++) {
				parentGeneration[i] = new List<SensorCam>(initialCams);
				childGeneration[i] = new List<SensorCam>(childFactor * genCount);
			}
			Random rand = new Random();

			for (int s = 0; s < steps; s++) {
				Program.Log("Starting with generation " + (s + 1) + "...");
				if (!UserInterface.OptimizeWindow.Running) break;
				progCB(s * 100 / steps, 0);
				for (int g = 0; g < groups; g++) {
					if (!UserInterface.OptimizeWindow.Running) break;
					// Reproduction + Mutation
					childGeneration[g].Clear();
					foreach (SensorCam parent in parentGeneration[g]) {
						//Program.Log((g+1)+") Parent: "+parent.X+"/"+parent.Y);
						for (int i = 0; i < childFactor; i++) {
							int x, y;

							do {
								x = parent.X + rand.Next(-sigma[g][0], sigma[g][0]);
								y = parent.Y + rand.Next(-sigma[g][1], sigma[g][1]);
								//Program.Log((g + 1) + ") Child: " + x + "/" + y);
							} while (!IsValidPosition(new HCoord(x, y)));

							childGeneration[g].Add(new SensorCam(x, y, parent.Orientation + rand.Next(-sigma[g][2], sigma[g][2])));
						}
					}

					// Selection
					bool foundBetter = false;
					for (int i = 0; i < childFactor; i++) {
						if (!UserInterface.OptimizeWindow.Running) break;
						List<SensorCam> tempParents = new List<SensorCam>(genCount);
						for (int j = 0; j < genCount; j++) {
							int index = j * childFactor + i;
							SensorCam cam = childGeneration[g].ElementAt<SensorCam>(index);
							tempParents.Add(cam);
						}

						//Program.Log((g+1)+") Testing combination " + (i + 1) + " of " + childFactor + "...");

						int genProg = (g*childFactor+i+1) * 100 / (groups*childFactor);
						progCB((s * 100 + genProg) / steps, genProg);
						int coverageValue = CalcCoverageValue(CalcCoverageMap(tempParents, roi), roi);
						//Program.Log("Value in group " + (g + 1) + ": " + coverageValue + " | " + tempParents[0].X + "/" + tempParents[0].Y + " " + tempParents[1].X + "/" + tempParents[1].Y + " " + tempParents[2].X + "/" + tempParents[2].Y);
						if (coverageValue > refValue[g]) {
							foundBetter = true;
							refValue[g] = coverageValue;
							parentGeneration[g] = new List<SensorCam>(tempParents);
							Program.Log("Found a new best in group " + (g + 1) + ": " + refValue[g] + " !");
							succCB(refValue[g]);
						}

					}
					if (foundBetter) {
						for(int i=0; i<3; i++)
							sigma[g][i] = Math.Max(10, sigma[g][i]/2);
					} else {
						for (int i = 0; i < 3; i++)
							sigma[g][i] = Math.Min(50, sigma[g][i] + 5);
					}
				}
			}
			progCB(100, 100);

			// Find best group
			int best = 0;
			int bestValue = 0;
			for (int i = 0; i < groups; i++) {
				if (bestValue < refValue[i]) {
					best = i;
					bestValue = refValue[i];
				}
			}

			// Update list
			for (int i = 0; i < genCount; i++) {
				SensorCam cam = initialCams[i];
				cam.X = parentGeneration[best][i].X;
				cam.Y = parentGeneration[best][i].Y;
				cam.Orientation = parentGeneration[best][i].Orientation;
			}
			Program.Log("Finished optimizing with " + bestValue);
			Program.CamResume();

		}

	}
}
